#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include <mosquitto.h>
#include <string>
#include <thread>
#include <atomic>

class MqttBridgeNode : public rclcpp::Node
{
public:
    MqttBridgeNode() : Node("mqtt_bridge_node")
    {
        // 声明配置参数
        declare_parameter("mqtt.host", "localhost");
        declare_parameter("mqtt.port", 1883);
        declare_parameter("ros2_to_mqtt.topic_ros", "ros2_send_topic");
        declare_parameter("ros2_to_mqtt.topic_mqtt", "mqtt/receive");
        declare_parameter("mqtt_to_ros2.topic_mqtt", "mqtt/send");
        declare_parameter("mqtt_to_ros2.topic_ros", "ros2_receive_topic");

        // 获取配置参数
        get_parameters();

        // 初始化MQTT
        mosquitto_lib_init();
        mqtt_client_ = mosquitto_new(nullptr, true, this);
        if (!mqtt_client_) {
            RCLCPP_ERROR(get_logger(), "Failed to create MQTT client");
            return;
        }

        // 设置MQTT回调
        mosquitto_message_callback_set(mqtt_client_, on_mqtt_message);
        mosquitto_connect_callback_set(mqtt_client_, on_connect);
        mosquitto_disconnect_callback_set(mqtt_client_, on_disconnect);

        // 启动连接线程
        connect_thread_ = std::thread(&MqttBridgeNode::connect_loop, this);

        // 启动MQTT循环
        if (mosquitto_loop_start(mqtt_client_) != 0) {
            RCLCPP_ERROR(get_logger(), "Failed to start MQTT loop");
            return;
        }

        // 创建ROS 2订阅者(ROS->MQTT)
        ros_subscriber_ = create_subscription<std_msgs::msg::String>(
            ros2_to_mqtt_topic_ros_,
            10,
            std::bind(&MqttBridgeNode::ros_message_callback, this, std::placeholders::_1)
        );

        // 创建ROS 2发布者(MQTT->ROS)
        ros_publisher_ = create_publisher<std_msgs::msg::String>(
            mqtt_to_ros2_topic_ros_,
            10
        );

        RCLCPP_INFO(get_logger(), "MQTT Bridge initialized successfully");
    }

    ~MqttBridgeNode()
    {
        stop_connect_thread_ = true;
        if (connect_thread_.joinable()) {
            connect_thread_.join();
        }

        if (mqtt_client_) {
            mosquitto_loop_stop(mqtt_client_, true);
            mosquitto_disconnect(mqtt_client_);
            mosquitto_destroy(mqtt_client_);
        }
        mosquitto_lib_cleanup();
    }

private:
    void get_parameters()
    {
        mqtt_host_ = get_parameter("mqtt.host").as_string();
        mqtt_port_ = get_parameter("mqtt.port").as_int();
        ros2_to_mqtt_topic_ros_ = get_parameter("ros2_to_mqtt.topic_ros").as_string();
        ros2_to_mqtt_topic_mqtt_ = get_parameter("ros2_to_mqtt.topic_mqtt").as_string();
        mqtt_to_ros2_topic_mqtt_ = get_parameter("mqtt_to_ros2.topic_mqtt").as_string();
        mqtt_to_ros2_topic_ros_ = get_parameter("mqtt_to_ros2.topic_ros").as_string();
    }

    // ROS消息回调(转发到MQTT)
    void ros_message_callback(const std_msgs::msg::String::SharedPtr msg)
    {
        if (!is_connected_) {
            RCLCPP_WARN(get_logger(), "Not connected to MQTT broker, cannot publish");
            return;
        }

        int rc = mosquitto_publish(
            mqtt_client_,
            nullptr,
            ros2_to_mqtt_topic_mqtt_.c_str(),
            msg->data.length(),
            msg->data.c_str(),
            0,
            false
        );

        if (rc != 0) {
            RCLCPP_ERROR(get_logger(), "MQTT publish failed: %s", mosquitto_strerror(rc));
        } else {
            RCLCPP_DEBUG(get_logger(), "Sent to MQTT: %s", msg->data.c_str());
        }
    }

    // 处理MQTT消息(转发到ROS)
    void handle_mqtt_message(const std::string& topic, const std::string& payload)
    {
        if (topic != mqtt_to_ros2_topic_mqtt_) {
            return; // 只处理配置的MQTT主题
        }

        auto msg = std_msgs::msg::String();
        msg.data = payload;
        ros_publisher_->publish(msg);
        RCLCPP_DEBUG(get_logger(), "Received from MQTT: %s", payload.c_str());
    }

    // MQTT连接回调
    static void on_connect(struct mosquitto* mosq, void* obj, int rc)
    {
        auto* node = static_cast<MqttBridgeNode*>(obj);
        if (!node) return;

        if (rc == 0) {
            node->is_connected_ = true;
            RCLCPP_INFO(node->get_logger(), "Connected to MQTT broker");
            // 订阅配置的MQTT主题
          //  mosquitto_subscribe(mosq, nullptr, node->mqtt_to_ros2_topic_mqtt_.c_str(), 0);
        } else {
            node->is_connected_ = false;
            RCLCPP_ERROR(node->get_logger(), "MQTT connect failed: %s", mosquitto_strerror(rc));
        }
    }

    // MQTT断开连接回调
    static void on_disconnect(struct mosquitto* mosq, void* obj, int rc)
    {
        auto* node = static_cast<MqttBridgeNode*>(obj);
        if (node && rc != 0) {
            node->is_connected_ = false;
            RCLCPP_WARN(node->get_logger(), "Disconnected from MQTT broker, reconnecting...");
        }
    }

    // MQTT消息回调
    static void on_mqtt_message(struct mosquitto* mosq, void* obj, const struct mosquitto_message* msg)
    {
        if (!obj || !msg->payload) return;

        auto* node = static_cast<MqttBridgeNode*>(obj);
        std::string payload(static_cast<char*>(msg->payload), msg->payloadlen);
        std::string topic(msg->topic);
        node->handle_mqtt_message(topic, payload);
    }

    // MQTT重连循环
    void connect_loop()
    {
        int retry_delay = 1;
        const int max_delay = 10;

        while (rclcpp::ok() && !stop_connect_thread_) {
            if (!is_connected_) {
                RCLCPP_INFO(get_logger(), "loop Connecting to MQTT broker: %s:%d", 
                           mqtt_host_.c_str(), mqtt_port_);

                int rc = mosquitto_connect(mqtt_client_, mqtt_host_.c_str(), mqtt_port_, 60);
                if (rc != 0) {
                    RCLCPP_ERROR(get_logger(), "Connection failed: %s. Retrying in %ds...",
                                mosquitto_strerror(rc), retry_delay);
                    std::this_thread::sleep_for(std::chrono::seconds(retry_delay));
                    retry_delay = std::min(retry_delay * 2, max_delay);
                }
            } else {
                std::this_thread::sleep_for(std::chrono::seconds(1));
                retry_delay = 1; // 重置重连延迟
            }
        }
    }

    // MQTT相关
    struct mosquitto* mqtt_client_ = nullptr;
    std::string mqtt_host_;
    int mqtt_port_;
    std::atomic<bool> is_connected_{false};
    std::thread connect_thread_;
    std::atomic<bool> stop_connect_thread_{false};

    // 主题配置
    std::string ros2_to_mqtt_topic_ros_;    // ROS发送主题
    std::string ros2_to_mqtt_topic_mqtt_;   // MQTT接收主题
    std::string mqtt_to_ros2_topic_mqtt_;   // MQTT发送主题
    std::string mqtt_to_ros2_topic_ros_;    // ROS接收主题

    // ROS 2相关
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr ros_subscriber_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr ros_publisher_;
};

int main(int argc, char* argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<MqttBridgeNode>());
    rclcpp::shutdown();
    return 0;
}